import lejos.nxt.Button;
import lejos.nxt.LightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.Sound;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.CompassSensor;
import lejos.nxt.comm.RConsole;
import lejos.robotics.navigation.CompassPilot;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

public class PathFollower {
    public static enum State {
	GO, AVOID_L, AVOID_R, BUMP_L, BUMP_R, STOP
    }

    private static final float WHEEL_DIA = 3.25f;
    private static final float TRACK_WIDTH = 13f;

    private static final int MIN_DISTANCE = 10;
    private static final float MAX_RAD = 15f;
    private static final float MIN_AVOID_ANGLE = 45;

    private static final int MIN_LIGHT = 50;
    private static final float BUMP_ANGLE = 25f;
    private static final float TURN_RATE = 200;

    private static final int LIGHT_FREQ = 1000;
    private static final int DIST_FREQ = 2000;
    private static final int TONE_DURATION = 250;
    private static final int TONE_VOLUME = 50;

    private LightSensor lLight;
    private LightSensor rLight;
    private Motor lMotor;
    private Motor rMotor;
    private UltrasonicSensor sonic;
    private State state;
    private Pilot pilot;
    private StringBuffer debug = new StringBuffer();

    public PathFollower(SensorPort ultrasonicPort, SensorPort leftLightPort,
	    SensorPort rightLightPort, Motor leftMotor, Motor rightMotor) {
	lLight = new LightSensor(leftLightPort);
	rLight = new LightSensor(rightLightPort);
	lMotor = leftMotor;
	rMotor = rightMotor;
	sonic = new UltrasonicSensor(ultrasonicPort);
	state = State.GO;

	pilot = new TachoPilot(WHEEL_DIA, TRACK_WIDTH, lMotor, rMotor);
    }

    public PathFollower(SensorPort ultrasonicPort, SensorPort leftLightPort,
	    SensorPort rightLightPort, Motor leftMotor, Motor rightMotor,
	    SensorPort compassPort) {
	lLight = new LightSensor(leftLightPort);
	rLight = new LightSensor(rightLightPort);
	lMotor = leftMotor;
	rMotor = rightMotor;
	sonic = new UltrasonicSensor(ultrasonicPort);
	state = State.GO;

	pilot = new CompassPilot(new CompassSensor(compassPort), WHEEL_DIA,
		TRACK_WIDTH, lMotor, rMotor);
    }

    public int calibrateOnPath() {
	lLight.calibrateLow();
	rLight.calibrateLow();
	debug.append("LLow:  ");
	debug.append(lLight.getLow());
	debug.append("\nRLow:  ");
	debug.append(rLight.getLow());
	RConsole.println(debug.toString());
	debug.delete(0, debug.length());
	return (lLight.getLow() + rLight.getLow()) / 2;
    }

    public int calibrateOffPath() {
	lLight.calibrateHigh();
	rLight.calibrateHigh();
	debug.append("LHigh: ");
	debug.append(lLight.getHigh());
	debug.append("\nRHigh: ");
	debug.append(rLight.getHigh());
	RConsole.println(debug.toString());
	debug.delete(0, debug.length());
	return (lLight.getHigh() + rLight.getHigh()) / 2;
    }

    public boolean calibrateCompass() {
	if (pilot instanceof CompassPilot) {
	    ((CompassPilot) pilot).calibrate();
	    return true;
	}
	return false;
    }

    public void setSpeed(float speed) {
	pilot.setSpeed((int) speed);
    }

    private static boolean onPath(LightSensor sensor) {
	int lightValue = sensor.getLightValue();
	if (onPath(sensor.getLightValue())) {
	    Sound.playTone(LIGHT_FREQ - lightValue, TONE_DURATION, TONE_VOLUME);
	    return true;
	}
	return false;
    }

    private static boolean onPath(int lightValue) {
	return lightValue <= MIN_LIGHT;
    }

    private boolean nearObject(int distance) {
	return distance <= MIN_DISTANCE;
    }

    private void avoid(boolean avoidRight) {
	long startTime = System.currentTimeMillis();
	int toggle = avoidRight ? -1 : 1;

	// Back up to achieve minimum distance from object
	RConsole.println("->backing away from object");
	pilot.backward();
	while (nearObject(sonic.getDistance()))
	    ;

	// Turn left and begin arc
	RConsole.println("->rotating and beginning arc");
	pilot.rotate(toggle * 90f);
	pilot.reset();
	pilot.arc(-toggle * (MAX_RAD + MIN_DISTANCE), 360f, true);

	// Arc for a minimum amount to ensure we are passed the path we just
	// left
	RConsole.println("->moving past path");
	while (Math.abs(pilot.getAngle()) < MIN_AVOID_ANGLE)
	    ;

	// Arc until one of the sensors finds the path
	RConsole.println("->searching for path");
	while (!onPath(rLight) && !onPath(lLight))
	    ;

	// If right sensor found path, wait for left to find it and
	// vice-versa
	RConsole.println("->waiting for other sensor to find path");
	if (onPath(rLight)) {
	    while (!onPath(lLight))
		;
	} else {
	    while (!onPath(rLight))
		;
	}

	// Keep arcing until both sensors passed the path
	RConsole.println("->waiting for both sensors to cross path");
	while (onPath(rLight) || onPath(lLight))
	    ;

	// Rotate left until the path is re-acquired with right sensor
	// on the path
	RConsole.println("->re-acquiring path");
	pilot.rotate(toggle * 180f, true);
	while (onPath(lLight) || !onPath(rLight))
	    ;
	RConsole.println("->continuing path traversal");
	RConsole.println("->object avoidance took "
		+ (System.currentTimeMillis() - startTime) / 1000. + '\n');

    }

    private State updateState() {
	int lLightValue = lLight.getLightValue();
	int rLightValue = rLight.getLightValue();
	int distance = sonic.getDistance();

	debug.append("Left:  ");
	debug.append(lLightValue);
	debug.append("\nRight: ");
	debug.append(rLightValue);
	debug.append("\nDist:  ");
	debug.append(distance);
	debug.append("\n");

	if (distance != 255)
	    Sound.playTone(DIST_FREQ - distance, TONE_DURATION, TONE_VOLUME);

	if (onPath(lLightValue) && onPath(rLightValue))
	    return (state = State.STOP);
	else if (nearObject(distance)) {
	    if (onPath(lLightValue))
		return (state = State.AVOID_L);
	    else if (onPath(rLightValue))
		return (state = State.AVOID_R);
	    else
		return (state = Math.random() < .5 ? State.AVOID_L
			: State.AVOID_R);
	} else if (onPath(lLightValue))
	    return (state = State.BUMP_L);
	else if (onPath(rLightValue))
	    return (state = State.BUMP_R);
	else
	    return (state = State.GO);
    }

    public void run() {
	while (Button.ESCAPE.isPressed() || updateState() != State.STOP) {
	    debug.append("State: ");
	    debug.append(state.toString());
	    debug.append('\n');
	    if (state != State.GO)
		RConsole.println(debug.toString());
	    debug.delete(0, debug.length());

	    switch (state) {
	    case GO:
		pilot.forward();
		break;
	    case AVOID_R:
		// true should be passed but do to the fact that the robot
		// naturally turns to the right as a result of some mechanical
		// malfunction, it can't arc to the left
		avoid(false);
		break;
	    case AVOID_L:
		avoid(false);
		break;
	    case BUMP_L:
		pilot.steer(TURN_RATE, BUMP_ANGLE);
		break;
	    case BUMP_R:
		pilot.steer(-TURN_RATE, BUMP_ANGLE);
		break;
	    }
	}
	debug.append("State: ");
	debug.append(state.toString());
	debug.append("\n");
	RConsole.println(debug.toString());
	pilot.stop();
    }
}
